/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Servo Ctrl
 */
#ifndef SERVO_CTRL_H_
#define SERVO_CTRL_H_

#include "ros/ros.h"

#include "std_msgs/String.h"
#include "std_msgs/UInt16.h"
#include "geometry_msgs/Vector3.h"
#include "group6/transformCameraServo.h"
#include "group6/transformManipulatorServo.h"
#include "group6/healthCheck.h"

/**
 * The ServoCtrl class handles the actuator controls for the pan/tilt of the
 * kinect camera as well as the manipulator.
 * The control of the servos is done through an arduino client board, which
 * is commanded by rosserial.
 * The following ports are mapped on the arduino:
 * 8: tilt of kinect
 * 9: pan of kinect
 * 10: grab using manipulator claw
 * 11: raise and lower manipulator
 * 12: not connected (yet)
 * 13: not connected (yet)
 */
class ServoCtrl 
{
    public:

        enum servoTypes {
            SERVO_PTU = 1,
            SERVO_MANIP = 2
        };

        enum healthTypes { NOT_READY = 0,
            INIT_SUCCESSFULL = 1,
            INIT_NOT_SUCCESSFULL = 2,
            SERVO_ERROR = 3,
            ACTION_ERROR = 4,
            SERVO_SUCCESS = 5
        };

        enum actionTypes {
            PAN_CAMERA = 1,
            TILT_CAMERA = 2,
            RAISE_MANIP = 3,
	    HOVER_MANIP = 7,
            LOWER_MANIP = 4,
            GRAB = 5,
            RELEASE = 6
        };

        ServoCtrl(ros::NodeHandle& nh);
        ~ServoCtrl(){}
        bool transformCamera(group6::transformCameraServo::Request &req, group6::transformCameraServo::Response &res);
        bool transformManipulator(group6::transformManipulatorServo::Request &req, group6::transformManipulatorServo::Response &res);
        bool healthCheck(group6::healthCheck::Request &req, group6::healthCheck::Response &res);

        // getters, setters
        void setHealth(int h);

    private:

        bool runServo(int servo);

        ros::Publisher ptu_servo_pub;
        ros::Publisher manip_servo_pub;
        ros::ServiceServer health_check_service;
        ros::ServiceServer servo_camera_service;
        ros::ServiceServer servo_manipulator_service;

        ros::NodeHandle mNodeHandle;

        double cameraPanAngle;
        double cameraTiltAngle;
        double gripperAngle;
        double liftAngle; 
        int health;
};

#endif
